import numpy as np
import Common.MatrixProcess as mp
from Common.ControlEnum import forcemode
class AddmittanceControl():
    def __init__(self,k3,b3,m3,f3,dt,trans=True,teachmode=forcemode.FreeMode):
        self.k=k3
        self.b=b3
        self.m=m3
        self.dt=dt
        self.trans=trans
        self.teachmode=teachmode
        print(f'导纳算法刚度矩阵k={self.k},b={self.b},m={self.m},dt={self.dt},是否转换力坐标：{trans}')

        self.goalFt = f3#工具坐标系下的f3
        self.times = 6170
        self.i = 0
        self.delta_pose_t0=np.zeros((3))
        self.delta_vel_t0 = np.zeros((3))
        self.delta_acc_t0 = np.zeros((3))

        self.delta_pose_t1=np.zeros((3))
        self.delta_vel_t1 = np.zeros((3))
        self.delta_acc_t1 = np.zeros((3))
        self.goalpose_old = np.zeros((6))
        self.goalpose_v = np.zeros((3))

        #lisen 添加 start
        self.goalpose_v_old = np.zeros((3))
        self.goalpose_a = np.zeros((3))

        self.i

        #region 力模式初始化
        if (teachmode == forcemode.TeachMode):
            self.saveforce = []#示教力
        elif (teachmode==forcemode.ReplayMode):
            self.readforce=[]#复现示教的力
            self.readforce=np.loadtxt("demoforce.txt")
        else:
            None
        #endregion

    def get_save_value(self):
        return self.saveforce

    def forceTrans(self,istrans,pose,force):
        if(istrans==True):
            RBase_Tcp = mp.rotation_matrix_from_vectorr(pose[3:])
            force_base_cur = RBase_Tcp.dot(force[:3])
            return force_base_cur
        else:
            return force

    def forceCal(self,teachmode,curforce):
        """
        根据模式，选择是复现力，还是记录力
        Args:
            teachmode:
            force:当前的力

        Returns:

        """
        if(teachmode==forcemode.TeachMode):
            Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
            self.saveforce.append(curforce[:3])
        if(teachmode==forcemode.FreeMode):
            Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
        if(teachmode==forcemode.ReplayMode):
            curforce[:2] = 0.1*curforce[:2] + self.readforce[self.i][:2]
            Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b - (self.m - self.b * self.dt) * self.goalpose_a[:3]
        return Fe

    def calpose(self,curpose,goalpose,curforce):
        """
        Args:
            curpose:当前位置，从机器人读取上来的
            goalpose: 目标轨迹，世界坐标系下的
            curvel:
            curforce:

        Returns:
        """

        curforce=self.forceTrans(self.trans,curpose,curforce)
        if(self.i ==0):
            self.goalpose_v=(goalpose - curpose)/self.dt  #给速度初始化,只执行一次
            self.goalpose_a = 2*(goalpose[:3]-curpose[:3])/self.dt/self.dt# 加速度初始化 用x=1/2*a*t^2计算

            #lisen 添加 end
        else:
            self.goalpose_v= (goalpose - self.goalpose_old)/self.dt
            self.goalpose_a = (self.goalpose_v - self.goalpose_v_old) / self.dt


        self.goalpose_old = goalpose.copy()
        self.goalpose_v_old = self.goalpose_v.copy()
        #lisen 添加 start

        #lisen添加 end
        curforce[np.abs(curforce)<2]=0
        if(abs(curforce[2])<2):
            curforce[2]=0
        if (abs(curforce[1]) < 0.5*abs(curforce[2])):
            curforce[1] = 0
        if(abs(curforce[0])<0.5*abs(curforce[2])):
            curforce[0]=0
        # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-self.m * (2*self.goalpose_a[:3] - 0.002)
        Fe=self.forceCal(self.teachmode,curforce)
        #示教模式
        # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*0.002)*self.goalpose_a[:3]
        # self.saveforce.append(curforce[:3])
        # #复现示教力
        # curforce[:2]=curforce[:2]+self.readforce[self.i][:2]
        # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b - (self.m - self.b * self.dt) * self.goalpose_a[:3]


        # Fe = curforce[:3] - self.goalFt + [0, 0, 0.00002*0.0001/0.002  * 420*self.times]+[0,0,(0.00002*0.0001/0.002/0.002 -0.002)* 10]#time时间次数递增
        # self.times = 1+self.times
        # 传感器上的力转换为基坐标上
        # RTcp_in_base = mp.create_transformation_matrix(curpose)[:3, :3]
        # Fe = np.reshape(Fe, (3, 1))
        # Fe = RTcp_in_base.dot(Fe)
        # Fe = np.reshape(Fe,(1,3))
        # Fe=np.array([0,0,0])
        #Ve=curvel[:3]#实际速度与理想速度之差
        #Xe=curpose[:3]-goalpose[:3]#实际位置与理想位置之差

        # 2 导纳公式
        Ve=self.delta_vel_t0
        Xe=self.delta_pose_t0
        self.delta_acc_t1 = (1 / self.m) * (Fe - self.b * Ve)

        #lisen 添加 start
        # self.delta_acc_t1= (1 / self.m) * (Fe - self.b * Ve - (self.m - self.b*0.002)*self.goalpose_a)    #求加速度的差值  在参考轨迹加速度为零情况下
        #lisen 添加 end

        self.delta_vel_t1= Ve + self.delta_acc_t1 * self.dt

        sump = np.linalg.norm(self.delta_vel_t1)
        if (sump > 0.1):
            self.delta_vel_t1 = (0.0006 / sump) * self.delta_vel_t1

        self.delta_pose_t1= Xe + self.delta_vel_t1 * self.dt#x0+vt

        self.delta_pose_t0=self.delta_pose_t1
        self.delta_vel_t0=self.delta_vel_t1
        # RBase_Tcp = mp.create_transformation_matrix(curpose)[:3, :3]
        # deltapose = np.reshape(self.delta_pose_t1, (3, 1))
        # deltapose = RBase_Tcp.dot(deltapose)
        # deltapose = np.reshape(deltapose, (1, 3))

        movepose=np.zeros(6)
        movepose[:3]=goalpose[:3] #最终是加在goalpose上面了
        movepose[2]+= self.delta_pose_t1[2]
        movepose[0] += self.delta_pose_t1[0]
        movepose[1] += self.delta_pose_t1[1]
        #print("delta_pose_t1=",self.delta_pose_t1)
        movepose[3:]=goalpose[3:]
        self.i = self.i + 1
        return movepose#这个是最终的坐标位置，基坐标系下的

# A=AddmittanceControl()